#include "i2c.h"

extern float pitch_data;    //俯仰角
extern sem_t sem_iic;

int i2c_init(void)
{
    int fd;
    char addr = 0x50;
    char pitch_buff[2] = {0};
    float pitch;
    // // char roll_data[1] = {0};
    fd = open("/dev/i2c-1",O_RDWR);
    if( fd < 0 ){
        printf("i2c-1 file open error!\n");
        exit(-1);
    }
    // int ret = ioctl(fd, I2C_SLAVE, addr);
    // if( ret < 0 ){
    //     printf("fail to set i2c addr\n");
    //     close(fd);
    //     exit(-1);
    // }
    // ioctl(fd, I2C_TIMEOUT,10);
    // ioctl(fd, I2C_RETRIES,2);

    // i2c_read2(fd, 0x3E, roll_data);
    /*************************************************/
    // i2c_read(fd, 0x50, 0x3D, roll_data, 2);
    /*************************************************/
    // printf("%x ",roll_data[0]);
    // printf("%x \n",roll_data[1]);

    // usleep(300*1000);
    // int i;
    while(1){
        // P操作 等待信号 -1
        sem_wait(&sem_iic);
        
        /*************************************************/
        // i2c_read2(fd, 0x3E, roll_data);
        /*************************************************/
        if( ioctl(fd, I2C_SLAVE, addr) < 0 ){
            printf("fail to set i2c addr\n");
            close(fd);
            exit(-1);
        }
        ioctl(fd, I2C_TIMEOUT,10);
        ioctl(fd, I2C_RETRIES,2);
        i2c_read(fd, 0x50, 0x3E, pitch_buff, 2);        
        /*************************************************/
        pitch = ((short)pitch_buff[1]<<8|(short)pitch_buff[0])/32768.0*180.0;
        if(pitch > 180.0 ){
            pitch_data = pitch-360;
        }
        else{
            pitch_data = pitch;
        }
        // V +1等待main发送读取数据命令
        // printf("11111111111\n");
        sem_post(&sem_iic);
        usleep(1*1000);
        // printf("roll = %f\n",pitch);
        // printf("%x ",pitch_buff[0]);
        // printf("%x \n",pitch_buff[1]);
        // usleep(300*1000);

    }
    close(fd);
    
}

int i2c_read(int fd, unsigned char dev_addr, unsigned char reg_addr, unsigned char *data_buf, int len)
{
	int ret = 0;
	unsigned char msg_buf[9];
	struct i2c_rdwr_ioctl_data data;
	struct i2c_msg messages[2];
 
	messages[0].addr = dev_addr;
	messages[0].flags = 0;
	messages[0].len = 1;
	messages[0].buf = &reg_addr;
 
	messages[1].addr=dev_addr;
	messages[1].flags = I2C_M_RD;
	messages[1].len = len;
	messages[1].buf = data_buf;
	data.msgs = messages;
	data.nmsgs = 2;
 
	if(ioctl(fd, I2C_RDWR, &data) < 0)
	{
		printf("I2C_RDWR err\n");
		return -1;
	}
	
	usleep(50);
 
	return 0;
}

// int i2c_read2(int fd, unsigned char reg, unsigned char *val)
// {
//     int ret;
//     for(ret=5; ret>0; ret--){
//         if(write(fd, &reg, 1)==1){
//             if(read(fd, val, 2) == 2){
//                 return 0;
//             }
//         }
//     }
//     return 1;
// }
